/*
  Copyright (c) 2012-2013  John Lee (j.y.lee@yeah.net)
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:

  * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.

  * Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in
    the documentation and/or other materials provided with the
    distribution.

  * Neither the name of the copyright holders nor the names of
    contributors may be used to endorse or promote products derived
    from this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.
*/

#define __PURE      __attribute__((error("pure virtual mothed called")))
#define __INLINE    __attribute__((always_inline)) inline
#define __WEAK      __attribute__((noinline, weak))
#define __NAKED     __attribute__((naked))

#if defined(__ARM_ARCH_6M__)
#define look        look_armv6m
#define __look_ns__ "11look_armv6m"
#elif defined(__ARM_ARCH_7M__)
#define look        look_armv7m
#define __look_ns__ "11look_armv7m"
#else
#error This file must only be compiled for ARMv6-M or ARMv7-M architecture.
#endif

#include <cstdint>

__asm__ __volatile__(
    ".version \"LOOK (c) John Lee (j.y.lee@yeah.net)\"" "\n\t"
    ".global _ZN" __look_ns__ "7versionE"               "\n\t"
    ".weak _ZN" __look_ns__ "7versionE"                 "\n\t"
    ".set _ZN" __look_ns__ "7versionE, 0x20130310"
);

#define LOOK_SVC(_N, _R, _F, ...)       \
    ({                                  \
        register auto f asm ("ip") = reinterpret_cast<void (*)()>(_F);  \
        register _R result asm ("r0");  \
        __asm__ __volatile__(           \
            "svc %[n]"  "\n\t"          \
            : "=r" (result)             \
            : [n] "i" (_N), "r" (f), ##__VA_ARGS__  \
        );                              \
        result;                         \
    })

extern "C" void __libc_init_array();

extern "C" __WEAK __NAKED void __cxa_pure_virtual()
{
    while (true);
}

__INLINE void* operator new(uintptr_t, void* __p)
{
    return __p;
}

struct __ram_region_t {
    uint64_t* init;
    uint64_t* data;
    uint64_t* data_end;
    uint64_t* bss_end;
};

extern void (*__init_array_start)();
extern void (*__init_array_end)();
extern const __ram_region_t __ram_regions[];
extern uintptr_t __ram_region_num;

namespace look {
#if defined(__ARM_ARCH_7M__)
    template<typename D, typename T>
    __INLINE static void __ldrex(D& data, T& addr)
    {
        static_assert(sizeof(T) == sizeof(uint32_t) || sizeof(T) == sizeof(uint16_t) || sizeof(T) == sizeof(uint8_t), "the size of `addr' parameter type must be equal to 1 or 2 or 4");
        if (sizeof(T) == sizeof(uint32_t))
            __asm__ __volatile__("ldrex %[r],[%[p]]" : [r] "=r" (data) : [p] "r" (&addr));
        else if (sizeof(T) == sizeof(uint16_t))
            __asm__ __volatile__("ldrexh %[r],[%[p]]" : [r] "=r" (data) : [p] "r" (&addr));
        else
            __asm__ __volatile__("ldrexb %[r],[%[p]]" : [r] "=r" (data) : [p] "r" (&addr));
    }

    template<typename D, typename T>
    __INLINE static uint32_t __strex(D data, T& addr)
    {
        static_assert(sizeof(T) == sizeof(uint32_t) || sizeof(T) == sizeof(uint16_t) || sizeof(T) == sizeof(uint8_t), "the size of `addr' parameter type must be equal to 1 or 2 or 4");
        uint32_t result;
        if (sizeof(T) == sizeof(uint32_t))
            __asm__ __volatile__("strex %[r],%[d],[%[p]]" : [r] "=&r" (result) : [d] "r" (data), [p] "r" (&addr));
        else if (sizeof(T) == sizeof(uint16_t))
            __asm__ __volatile__("strexh %[r],%[d],[%[p]]" : [r] "=&r" (result) : [d] "r" (data), [p] "r" (&addr));
        else
            __asm__ __volatile__("strexb %[r],%[d],[%[p]]" : [r] "=&r" (result) : [d] "r" (data), [p] "r" (&addr));
        return result;
    }

    __INLINE static void __clrex()
    {
        __asm__ __volatile__("clrex");
    }

    bool do_set_privilege(bool state)
    {
        uintptr_t ctrl;
        __asm__ __volatile__ ("mrs %[ctrl],CONTROL" : [ctrl] "=r" (ctrl));
        if (((ctrl & 1) != 0) != state) {
            ctrl ^= 1;
            __asm__ __volatile__("msr CONTROL,%[ctrl]" : : [ctrl] "r" (ctrl));
            return true;
        }
        return false;
    }
#endif

    class sink_t {
    public:
        virtual void on_signal(void* addr, uintptr_t signal);
    };

    void sink_t::on_signal(void* addr, uintptr_t signal)
    {
    }

    class interrupt_t : public sink_t {
    public:
        virtual bool isr(int vector) = 0;
        virtual void dsr(int vector, uint_fast16_t count) = 0;
    };

    namespace base {
        class sched_t;
    }

    class vector_t {
        friend base::sched_t;

    private:
        __NAKED __WEAK static void reset();
        __NAKED __WEAK static void libc_reset();
        __NAKED __WEAK static void dsr();
        __WEAK static void nmi() __attribute__((alias("_ZN" __look_ns__ "8vector_t3dsrEv")));
        __WEAK static void hard_fault() __attribute__((alias("_ZN" __look_ns__ "8vector_t3dsrEv")));
        static void vsr();
#ifdef __ARM_ARCH_7M__
        __WEAK static void mem_manage() __attribute__((alias("_ZN" __look_ns__ "8vector_t3dsrEv")));
        __WEAK static void bus_fault() __attribute__((alias("_ZN" __look_ns__ "8vector_t3dsrEv")));
        __WEAK static void usage_fault() __attribute__((alias("_ZN" __look_ns__ "8vector_t3dsrEv")));
        __WEAK static void debug_monitor() __attribute__((alias("_ZN" __look_ns__ "8vector_t3dsrEv")));
#endif  // __ARM_ARCH_7M__

        __INLINE static void call_dsr()
        {
            while (true) {
                uint_fast8_t v;
                vector_t* vector;
                uint_fast16_t count;
#if defined(__ARM_ARCH_6M__)
                __asm__ __volatile__("cpsid i");
                v = header;
                if (v == 0) {
                    __asm__ __volatile__("cpsie i");
                    return;
                }
                v -= 15;
                vector = &table[v];
                __asm__ __volatile__("" : "+r" (vector));
                header = vector->next;
                count = vector->count;
                vector->count = 0;
                __asm__ __volatile__("cpsie i");
#elif defined(__ARM_ARCH_7M__)
                do {
                    __ldrex(v, header);
                    if (v == 0) {
                        __clrex();
                        return;
                    }
                    v -= 15;
                    vector = &table[v];
                    __asm__ __volatile__("" : "+r" (vector));
                } while (__strex(vector->next, header));
                do {
                    __ldrex(count, vector->count);
                } while (__strex(0, vector->count));
#endif
                if (count != 0)
                    vector->interrupt->dsr(v - 1, count);
            }
        }

    private:
        union {
            struct {
                interrupt_t* interrupt;
                uint16_t count;
                uint8_t next;
            };
            uint64_t data;
        };

        __attribute__((aligned(4))) static vector_t table[];
        __attribute__((section(".vector"), used)) static void (*const vectors[])();
        static uint8_t header;
    };

    uint8_t vector_t::header;
    void vector_t::dsr()
    {
        while (true);
    }

    class timer_t {
        friend class timer_queue_t;
        friend class systick_t;

    public:
        __INLINE timer_t(sink_t& sink) : sink{ &sink }, next{ 0 } { }
        __INLINE bool is_active() const { return next != 0; }

    protected:
        uintptr_t count;
        sink_t* sink;
        timer_t* next;
        timer_t* prev;
    };

    class timer_queue_t {
    public:
        timer_t& enqueue(timer_t& timer)
        {
            timer_t* h = header;
            if (h == nullptr) {
                header = &timer;
                timer.next = &timer;
                timer.prev = &timer;
            } else {
                auto count = timer.count;
                do {
                    if (count < h->count) {
                        if (h == header)
                            header = &timer;
                        break;
                    }
                    h = h->next;
                } while (h != header);
                timer_t* prev = h->prev;
                prev->next = &timer;
                h->prev = &timer;
                timer.next = h;
                timer.prev = prev;
            }
            return timer;
        }

        timer_t& remove(timer_t& timer)
        {
            if (&timer == header) {
                timer_t* next = timer.next;
                if (next == &timer) {
                    header = nullptr;
                    return timer;
                }
                header = next;
            }
            timer_t* p = timer.prev;
            timer.next->prev = p;
            p->next = timer.next;
            timer.next = nullptr;
            return timer;
        }

        __INLINE timer_t* get_header()
        {
            return header;
        }

    protected:
        timer_t* header;
    };

    class systick_t : public interrupt_t {
    public:
        __INLINE uintptr_t get_jiffies()
        {
            return jiffies;
        }
        __INLINE intptr_t get_timer_remain(timer_t& timer) const
        {
            return timer.count - jiffies;
        }
        uintptr_t do_set_timer(timer_t& timer, uintptr_t interval);

    protected:
        bool isr(int vector) override;
        void dsr(int vector, uint_fast16_t count) override;

    private:
        uintptr_t jiffies;
        timer_queue_t wheel[8];
    };

    bool systick_t::isr(int vector)
    {
        return true;
    }

    void systick_t::dsr(int vector, uint_fast16_t count)
    {
        do {
            jiffies++;
            auto& queue = wheel[jiffies % 8];
            timer_t* timer;
            while ((timer = queue.get_header()) != nullptr && timer->count == jiffies) {
                timer = &queue.remove(*timer);
                timer->sink->on_signal(timer, 0);
            }
        } while (--count);
    }

    uintptr_t systick_t::do_set_timer(timer_t& timer, uintptr_t interval)
    {
        uintptr_t remain = 0;
        if (timer.next != nullptr) {
            remain = timer.count - jiffies;
            wheel[timer.count % 8].remove(timer);
        }
        if (interval != 0) {
            uintptr_t count = interval + jiffies;
            timer.count = count;
            wheel[count % 8].enqueue(timer);
        }
        return remain;
    }

    extern uintptr_t systick;
    __INLINE systick_t& get_systick()
    {
        return *reinterpret_cast<systick_t*>(&systick);
    }

    class sync_t;
    class idle_task_t;

    struct except_saved_t {
        union {
            uint32_t r0;
            uint32_t a1;
            sync_t* sync;
            const idle_task_t* task;
        };
        union {
            uint32_t r1;
            uint32_t a2;
        };
        union {
            uint32_t r2;
            uint32_t a3;
        };
        union {
            uint32_t r3;
            uint32_t a4;
        };
        union {
            uint32_t r12;
            uint32_t ip;
        };
        union {
            uint32_t r14;
            uint32_t lr;
        };
        union {
            uint32_t r15;
            void* routine;
            uint32_t pc;
        };
        uint32_t xpsr;
    };

    struct context_t {
        uintptr_t call_saved[8];
        except_saved_t except_saved;
    };

    class idle_task_t : public sink_t {
    public:
        enum {
            MIN_CONTEXT_SIZE = 16
        };

    protected:
        __INLINE idle_task_t() { }

    protected:
        void init(uintptr_t& context, void* routine);

    public:
        context_t* context;
        union {
            struct {
                uint16_t info16;
                uint8_t block_state;
            };
            uint32_t info32;
        };
    };

    extern idle_task_t idle_task;
    void idle_task_t::init(uintptr_t& context, void* routine)
    {
        context_t* p = reinterpret_cast<context_t*>(&context);
        this->context = p;
#if defined(__ARM_ARCH_6M__)
        this->info32 = 0;
#elif defined(__ARM_ARCH_7M__)
        this->info32 = 1;
#endif
        p->except_saved.task = this;
        p->except_saved.routine = routine;
        p->except_saved.xpsr = 0x01000000;  //  T=thumb mode
    }

    extern uintptr_t scheduler;

    struct scs_t {
        uint32_t reserved0[96];
        uint32_t ICER[8];
        uint32_t reserved1[832 - 96 - 8];
        uint32_t CPUID;
        uint32_t ICSR;
        uint32_t reserved2[4];
        uint8_t SHP[12];
    };

    namespace base {
        class task_t : public idle_task_t {
        public:
            __INLINE bool is_blocked()
            {
                return context != nullptr && block_state != 0;
            }

            bool do_wakeup();

        protected:
            void on_signal(void* addr, uintptr_t signal) override;
        };

        class sched_t : public sink_t {
            friend vector_t;
            friend sync_t;

        public:
            __INLINE static sched_t& get_scheduler()
            {
                return *reinterpret_cast<sched_t*>(&scheduler);
            }
            __INLINE task_t& get_task()
            {
                return *cur_task;
            }
            __INLINE uint_fast16_t get_lock() const
            {
                return lock_count;
            }
            bool do_reschedule();
            bool do_unlock_reschedule();

        protected:
            task_t* do_wakeup(sync_t& sync, task_t* task, uintptr_t result);

        protected:
            virtual task_t* dispatch() = 0;
            virtual task_t* ready(sync_t& sync, task_t* task = 0) = 0;
            virtual void block(sync_t& sync) = 0;

        private:
            static void svcall() __attribute__((alias("_ZN" __look_ns__ "4base7sched_t6pendsvEv")));
            __NAKED void do_startup();
            __NAKED static void pendsv();
            using svcfn_t = uintptr_t (uintptr_t, uintptr_t, uintptr_t, uintptr_t);

            __INLINE uintptr_t* pendsv(uintptr_t* sp)
            {
                uintptr_t ipsr;
                __asm__ __volatile__("mrs %[ipsr],IPSR" : [ipsr] "=r" (ipsr));
                if (ipsr == 11) {
                    register uintptr_t a1 asm ("a1");
                    register uintptr_t a2 asm ("a2");
                    register uintptr_t a3 asm ("a3");
                    register uintptr_t a4 asm ("a4");

                    auto svcfn = reinterpret_cast<svcfn_t*>(sp);
                    __asm__ __volatile__("ldmia %[svcfn],{r0-r3,%[svcfn]}" : [svcfn] "+r" (svcfn), "=r" (a1), "=r" (a2), "=r" (a3), "=r" (a4));
                    uintptr_t result = svcfn(a1, a2, a3, a4);
                    except_saved_t* esp = reinterpret_cast<except_saved_t*>(sp);
                    if (result != 0) {
                        esp->r0 = result;
                        if (uintptr_t(need_sched) <= lock_count)
                            return sp;
                    } else {
                        uintptr_t n = reinterpret_cast<uint8_t*>(esp->pc)[-2];
                        if (n == 0) {
                            esp->r0 = result;
                            return sp;
                        }
                        uintptr_t timeout = (&esp->a1)[n];
                        cur_task->block_state = timeout == 0 ? 1 : 2;
                        if (timeout != 0) {
                            get_systick().do_set_timer(*new (reinterpret_cast<uint8_t*>(sp) - __builtin_offsetof(context_t, except_saved) - sizeof(timer_t)) timer_t(*cur_task), timeout);
                        }
                        need_sched = -1;
                        block(*esp->sync);
                    }
                }
                register task_t* new_task asm ("r7") = cur_task;
                do {
                    if (lock_count == 0)
                        vector_t::call_dsr();
                    if (need_sched != 0)
                        new_task = dispatch();
                } while (__extension__(({
                    volatile scs_t* scs = reinterpret_cast<volatile scs_t*>(0xe000e000);
                    bool pendsv = (scs->ICSR & 0x10000000) != 0;        // SCB->ICSR & SCB_ICSR_PENDSVSET_Msk
                    if (pendsv)
                        scs->ICSR = 0x8000000;                          // SCB->ICSR = SCB_ICSR_PENDSVCLR_Msk;
                    pendsv;
                })));
                auto old_task = cur_task;
                if (new_task != old_task) {
#if defined(__ARM_ARCH_6M__)
                    sp -= 8;
                    auto task = new_task;
                    __asm__ __volatile__("" : "=r" (new_task));
                    old_task->context = reinterpret_cast<context_t*>(sp);
                    old_task->info16 = lock_count;
                    info = task->info16;
                    cur_task = task;
                    __asm__ __volatile__(
                        "mov r4,r8"         "\n\t"
                        "mov r5,r9"         "\n\t"
                        "mov r6,r10"        "\n\t"
                        "mov r7,r11"        "\n\t"
                        "stmia %[sp]!,{r4-r7}"
                        : [sp] "+r" (sp)
                        :
                        : "r4", "r5", "r6", "r7"
                    );
                    sp = reinterpret_cast<uintptr_t*>(task->context);
                    task->context = nullptr;
                    __asm__ __volatile__(
                        "ldmia %[sp]!,{r4-r7}"  "\n\t"
                        "mov r8,r4"             "\n\t"
                        "mov r9,r5"             "\n\t"
                        "mov r10,r6"            "\n\t"
                        "mov r11,r7"
                        : [sp] "+r" (sp)
                        :
                        : "r4", "r5", "r6", "r7", "r8", "r9", "r10", "r11"
                    );
                    sp += 4;
                    __asm__ __volatile__("msr PSP,%[sp]" : : [sp] "r" (sp));
#elif defined(__ARM_ARCH_7M__)
                    sp -= 4;
                    __asm__ __volatile__("stmdb %[sp]!,{r8-r11}" : [sp] "+r" (sp));
                    uintptr_t info = lock_count;
                    uintptr_t ctrl;
                    __asm__ __volatile__("mrs %[ctrl],CONTROL" : [ctrl] "=r" (ctrl));
                    __asm__ __volatile__(
                        "adcs %[info],%[info]"
                        : [info] "+r" (info)
                        : "r" (ctrl >> 1)
                    );
                    old_task->info16 = info;
                    old_task->context = reinterpret_cast<context_t*>(sp);
                    cur_task = new_task;
                    sp = reinterpret_cast<uintptr_t*>(new_task->context);
                    new_task->context = nullptr;
                    info = new_task->info16 / 2;
                    __asm__ __volatile__("msr CONTROL,%[ctrl]" : : [ctrl] "r" (new_task->info16 & 1));
                    __asm__ __volatile__("ldmia %[sp]!,{r8-r11}" : [sp] "+r" (sp));
                    sp += 4;
                    __asm__ __volatile__("msr PSP,%[sp]" : : [sp] "r" (sp));
#endif
                }
                return sp;
            }

        protected:
            task_t* cur_task;
            union {
                struct {
                    uint16_t lock_count;
                    int8_t need_sched;
                };
                uint32_t info;
            };
        };

        bool sched_t::do_reschedule()
        {
            need_sched = -1;
            return true;
        }

        bool sched_t::do_unlock_reschedule()
        {
            lock_count--;
            return do_reschedule();
        }

        task_t* sched_t::do_wakeup(sync_t& sync, task_t* task, uintptr_t result)
        {
            task = ready(sync, task);
            if (task != nullptr) {
                task->context->except_saved.r0 = result;
                uintptr_t block_state = task->block_state;
                task->block_state = 0;
                if (block_state == 2) {
                    timer_t* timer = &reinterpret_cast<timer_t*>(task->context)[-1];
                    get_systick().do_set_timer(*timer, 0);
                }
            }
            return task;
        }

        void sched_t::do_startup()
        {
            volatile scs_t* scs = reinterpret_cast<volatile scs_t*>(0xe000e000);
            scs->ICSR = 0x10000000;             // SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
            scs->SHP[7] = 0xff;                 // svcall
            scs->SHP[10] = 0xff;                // pendsv
            auto task = static_cast<task_t*>(&idle_task);
            cur_task = task;
            __asm__ __volatile__("msr PSP,%[sp]" : : [sp] "r" (&task->context->except_saved));
            lock_count = 0;
            __asm__ __volatile__("bx %[r]" : : [r] "r" (-3));
        }

        void sched_t::pendsv()
        {
            register uintptr_t* sp asm ("r3");
            __asm__ __volatile__("mrs %[sp],PSP" : [sp] "=r" (sp));
            sp -= 4;
            __asm__ __volatile__("stmia %[sp]!,{r4-r7}" : [sp] "+r" (sp));
            auto* sched = &get_scheduler();
            __asm__ __volatile__("" : "+r" (sched));
            sp = sched->pendsv(sp);
            sp -= 4;
            __asm__ __volatile__("ldmia %[sp]!,{r4-r7}" : [sp] "+r" (sp));
            __asm__ __volatile__("bx %[r]" : : [r] "r" (-3));
        }
    }

    namespace link {
        class task_t : public base::task_t {
            friend class task_queue_t;
        public:
            __INLINE task_t* get_next()
            {
                return next;
            }
            __INLINE task_t* get_prev()
            {
                return prev;
            }

        protected:
            task_t* next;
            task_t* prev;
        };

        class task_queue_t {
        public:
            task_t& enqueue(task_t& task)
            {
                if (header == nullptr) {
                    header = &task;
                    task.next = &task;
                    task.prev = &task;
                } else {
                    task_t* prev = header->prev;
                    prev->next = &task;
                    header->prev = &task;
                    task.next = header;
                    task.prev = prev;
                }
                return task;
            }
            task_t* remove(task_t* task)
            {
                do {
                    if (task != nullptr) {
                        if (task != header)
                            break;
                    } else {
                        task = header;
                        if (task == nullptr)
                            return task;
                    }
                    task_t* next = task->next;
                    if (next == task) {
                        header = nullptr;
                        return task;
                    }
                    header = next;
                } while (false);
                task_t* p = task->prev;
                task->next->prev = p;
                p->next = task->next;
                return task;
            }
            __INLINE task_t* dequeue()
            {
                return remove(0);
            }
            __INLINE task_t* get_header()
            {
                return header;
            }
            __INLINE task_t* rotate()
            {
                header = header->next;
                return header;
            }

        protected:
            __INLINE static task_t* get_next(task_t& task)
            {
                return task.next;
            }
            __INLINE static task_t* get_prev(task_t& task)
            {
                return task.prev;
            }
            __INLINE static void set_next(task_t& task1, task_t* task2)
            {
                task1.next = task2;
            }
            __INLINE static void set_prev(task_t& task1, task_t* task2)
            {
                task1.prev = task2;
            }

        protected:
            task_t* header;
        };
    }

    class sync_t {
        friend base::task_t;

    public:
        bool do_block();

    protected:
        __INLINE base::task_t* do_wakeup(uintptr_t data, base::task_t* task = nullptr)
        {
            return base::sched_t::get_scheduler().do_wakeup(*this, task, data);
        }

    public:
        union {
            uintptr_t wait_map;
            link::task_queue_t wait_queue;
        };
    };

    bool sync_t::do_block()
    {
        return false;
    }

    sync_t sync;

    class sem_t : public sync_t {
    public:

#if defined(__ARM_ARCH_7M__)
        __attribute__((alias("_ZN" __look_ns__ "5sem_t10do_trywaitEv"))) bool trywait();
#endif  // __ARM_ARCH_7M__
        bool do_trywait();
        bool do_post();

    private:
        uintptr_t count;
    };

    bool sem_t::do_trywait()
    {
#if defined(__ARM_ARCH_6M__)
        bool r = count != 0;
        count -= r;
        return r;
#elif defined(__ARM_ARCH_7M__)
        decltype(count) cnt;
        do {
            __ldrex(cnt, count);
            if (cnt == 0) {
                __clrex();
                return false;
            }
            cnt--;
        } while (__strex(cnt, count));
        return true;
#endif
    }

    bool sem_t::do_post()
    {
        intptr_t cnt;
        cnt = count;
        if (cnt == 0) {
            base::task_t* task = do_wakeup(1);
            if (task != nullptr)
                return true;
        }
        count = cnt + 1;
        return true;
    }

    namespace base {
        class mbox_t : public sync_t {
        protected:
            bool do_tryget(uintptr_t& msg);
            bool do_tryput(uintptr_t msg, bool front);

        private:
            void put_inner(uintptr_t msg, bool front)
            {
                if (!front) {
                    buffer[in] = msg;
                    in = (in + 1 == size) ? 0 : in + 1;
                    return;
                }
                uint_fast8_t _out = out;
                if (_out == 0)
                    _out = size;
                buffer[--_out] = msg;
                out = _out;
            }

        protected:
            uint8_t size;
            uint8_t in;
            uint8_t out;
            uint8_t entries;
            uintptr_t buffer[0];
        };

        bool mbox_t::do_tryget(uintptr_t& msg)
        {
            if (entries == 0)
                return false;
            msg = buffer[out];
            out = (out + 1 == size) ? 0 : out + 1;
            task_t* task;
            if (entries != size || (task = do_wakeup(1)) == nullptr)
                entries--;
            else
                put_inner(task->context->except_saved.a2, task->context->except_saved.a3);
            return true;
        }

        bool mbox_t::do_tryput(uintptr_t msg, bool front)
        {
            bool r = false;
            if (size - entries != 0) {
                task_t* task;
                if (entries == 0 && (task = do_wakeup(1)) != nullptr)
                    *reinterpret_cast<uintptr_t*>(task->context->except_saved.a2) = msg;
                else {
                    entries++;
                    put_inner(msg, front);
                }
                r = true;
            }
            return r;
        }

        class flag_t : public sync_t {
        public:
            enum mode_t {
                ANY_KEEP,
                ANY_CONSUME,
                ALL_KEEP,
                ALL_CONSUME,
                LSB_KEEP,
                LSB_CONSUME
            };

        public:
#if defined(__ARM_ARCH_7M__)
            __attribute__((alias("_ZN" __look_ns__ "4base6flag_t7do_pollEjNS1_6mode_tE"))) uintptr_t poll(uintptr_t pattern, mode_t mode);
#endif
            uintptr_t do_poll(uintptr_t pattern, mode_t mode)
            {
                uintptr_t flags_rdy;
#if defined(__ARM_ARCH_6M__)
                flags_rdy = flags & pattern;
                if (flags_rdy != 0) {
                    if (mode >= 4) {
                        flags_rdy &= -flags_rdy;
                    } else if (mode >= 2) {
                        if (flags_rdy != pattern) {
                            return 0;
                        }
                    }
                    if ((mode & 1) != 0) {
                        flags &= ~flags_rdy;
                    }
                }
                return flags_rdy;
#elif defined(__ARM_ARCH_7M__)
                while (true) {
                    uintptr_t _flags;
                    __ldrex(_flags, flags);
                    flags_rdy = _flags & pattern;
                    if (flags_rdy == 0)
                        break;
                    if (mode >= 4) {
                        flags_rdy &= -flags_rdy;
                    } else if (mode >= 2) {
                        if (flags_rdy != pattern) {
                            flags_rdy = 0;
                            break;
                        }
                    }
                    if ((mode & 1) == 0)
                        break;
                    if (__strex(_flags & ~flags_rdy, flags) == 0)
                        return flags_rdy;
                }
                __clrex();
                return flags_rdy;
#endif
            }

#if defined(__ARM_ARCH_7M__)
            __attribute__((alias("_ZN" __look_ns__ "4base6flag_t12do_mask_bitsEj"))) bool mask_bits(uintptr_t pattern);
#endif
            bool do_mask_bits(uintptr_t pattern);

        protected:
            uintptr_t flags;
        };

        bool flag_t::do_mask_bits(uintptr_t pattern)
        {
#if defined(__ARM_ARCH_6M__)
            flags &= pattern;
#elif defined(__ARM_ARCH_7M__)
            uintptr_t _flag;
            do {
                __ldrex(_flag, flags);
                _flag &= pattern;
            } while (__strex(_flag, flags));
#endif
            return true;
        }
    }

    namespace link {
        class flag_t : public base::flag_t {
        public:
            bool do_set_bits(uintptr_t pattern);
        };

        bool flag_t::do_set_bits(uintptr_t pattern)
        {
            flags |= pattern;
            task_t* task = wait_queue.get_header();
            if (task != nullptr) {
                task_t* tail = task->get_prev();
                while (flags != 0) {
                    task_t* next = task->get_next();
                    uintptr_t result = do_poll(task->context->except_saved.r1, mode_t(task->context->except_saved.r2));
                    if (result != 0)
                        do_wakeup(result, task);
                    if (task == tail)
                        break;
                    task = next;
                }
            }
            return true;
        }
    }

    __attribute__((constructor(105))) void init_ram_region()
    {
        uintptr_t num = reinterpret_cast<uintptr_t>(&__ram_region_num);
        register const __ram_region_t* region asm ("r0") = __ram_regions;
        do {
            register uint64_t* _init asm ("r4");
            register uint64_t* _data asm ("r5");
            register uint64_t* _edata asm ("r6");
            register uint64_t* ebss asm ("r7");
            __asm__ __volatile__(
                "ldmia %[r]!,{%[init],%[data],%[ed],%[eb]}"
                : [r] "+r" (region),
                    [init] "=r" (_init),
                    [data] "=r" (_data),
                    [ed] "=r" (_edata),
                    [eb] "=r" (ebss)
            );
            uint64_t* init = _init;
            uint64_t* data = _data;
            uint64_t* edata = _edata;
            while (data != edata) {
                *data++ = *init++;
                __asm__ __volatile__("" : "+r" (data), "+r" (init), "+r" (edata));
            }
            uint64_t zero = 0;
            while (data != ebss) {
                *data++ = zero;
                __asm__ __volatile__("" : "+r" (data), "+r" (ebss), "+r" (zero));
            }
        } while (--num != 0);
    }

    void vector_t::reset()
    {
        void (**p)() = &__init_array_start;
        void (**end)() = &__init_array_end;
        __asm__ __volatile__("" : "+r" (end));
        while (p != end) {
            (*p++)();
        }
        register uintptr_t sp asm ("sp");
        __asm__ __volatile__(
            "msr PSP,%[sp]"             "\n\t"
            "msr CONTROL,%[ctrl]"
            : : [sp] "r" (sp - 64), [ctrl] "r" (
#if defined(__ARM_ARCH_6M__)
                2
#elif defined(__ARM_ARCH_7M__)
                3
#endif
            )
        );
        register auto scheduler asm ("a1") = &base::sched_t::get_scheduler();
        LOOK_SVC(0, uintptr_t, &base::sched_t::do_startup, "0" (scheduler));
    }

    void vector_t::libc_reset()
    {
        __libc_init_array();
        register uintptr_t sp asm ("sp");
        __asm__ __volatile__(
            "msr PSP,%[sp]"             "\n\t"
            "msr CONTROL,%[ctrl]"
            : : [sp] "r" (sp - 64), [ctrl] "r" (
#if defined(__ARM_ARCH_6M__)
                2
#elif defined(__ARM_ARCH_7M__)
                3
#endif
            )
        );
        register auto scheduler asm ("a1") = &base::sched_t::get_scheduler();
        LOOK_SVC(0, uintptr_t, &base::sched_t::do_startup, "0" (scheduler));
    }

    void vector_t::vsr()
    {
        int vec;
        __asm__ __volatile__("mrs %[vec],IPSR" : [vec] "=r" (vec));
        vector_t& vector = table[vec - 15];
        if (vector.interrupt != 0) {
            if (vector.interrupt->isr(vec - 16)) {
                if (vector.count++ == 0) {
                    uintptr_t v;
#if defined(__ARM_ARCH_6M__)
                    __asm__ __volatile__("cpsid i");
                    v = header;
                    header = vec;
                    __asm__ __volatile__("cpsie i");
#elif defined(__ARM_ARCH_7M__)
                    do {
                        __ldrex(v, header);
                    } while (__strex(vec, header));
#endif
                    vector.next = v;
                }
                if (base::sched_t::get_scheduler().get_lock() == 0)
                    reinterpret_cast<volatile scs_t*>(0xe000e000)->ICSR = 0x10000000;   // SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
            }
        } else {
            vec -= 16;
#if defined(__ARM_ARCH_6M__)
            reinterpret_cast<volatile scs_t*>(0xe000e000)->ICER[0] = 1 << vec;
#elif defined(__ARM_ARCH_7M__)
            reinterpret_cast<volatile scs_t*>(0xe000e000)->ICER[uintptr_t(vec - 16) >> 5] = 1 << (vec & 0x1f);
#endif
        }
    }

    namespace base {
        bool task_t::do_wakeup()
        {
            if (is_blocked()) {
                context->except_saved.sync->do_wakeup(0, this);
                return true;
            }
            return false;
        }

        void task_t::on_signal(void* addr, uintptr_t signal)
        {
            if (context != nullptr && addr == reinterpret_cast<const uint8_t*>(context) - sizeof(timer_t))
                context->except_saved.sync->do_wakeup(0, this);
        }
    }

    namespace co {                      // Cooperative Scheduling
        class task_t : public link::task_t {
        };

        using link::task_queue_t;

        class sched_t : public base::sched_t {
        public:
            void create(task_t& task) __attribute__((alias("_ZN" __look_ns__ "2co7sched_t8add_taskERNS0_6task_tE")));

        protected:                      // virtual
            base::task_t* dispatch() override;
            base::task_t* ready(sync_t& sync, base::task_t* task = 0) override;
            void block(sync_t& sync) override;

        protected:
            task_t& add_task(task_t& task);
            bool do_yield(uintptr_t sched);

        protected:
            task_queue_t ready_queue;
        };

        base::task_t* sched_t::dispatch()
        {
            task_t* task = static_cast<task_t*>(ready_queue.get_header());
            return task == nullptr ? static_cast<task_t*>(&idle_task) : task;
        }

        base::task_t* sched_t::ready(sync_t& sync, base::task_t* task)
        {
            task = sync.wait_queue.remove(static_cast<task_t*>(task));
            return task == nullptr ? task : &add_task(*static_cast<task_t*>(task));
        }

        task_t& sched_t::add_task(task_t& task)
        {
            if (ready_queue.get_header() == nullptr)
                need_sched = 1;
            return static_cast<task_t&>(ready_queue.enqueue(task));
        }

        void sched_t::block(sync_t& sync)
        {
            sync.wait_queue.enqueue(*static_cast<task_t*>(ready_queue.remove(static_cast<task_t*>(cur_task))));
        }

        bool sched_t::do_yield(uintptr_t sched)
        {
            task_t* task = static_cast<task_t*>(ready_queue.get_header());
            if (task == cur_task) {
                task = static_cast<task_t*>(ready_queue.rotate());
                if (task != cur_task) {
                    need_sched = sched;
                    return true;
                }
            }
            return false;
        }
    }
    namespace edf {                     // Earliest Deadline First Scheduling
        class task_t : public link::task_t {
            friend class sched_t;
            friend class task_queue_t;

        protected:
            void init(uintptr_t& context, uintptr_t deadline);

        protected:
            uintptr_t deadline;
        };

        class task_queue_t : public link::task_queue_t {
            friend class sched_t;

        protected:
            task_t& insert(task_t& task)
            {
                task_t* t = static_cast<task_t*>(header);
                if (t == 0) {
                    header = &task;
                    set_next(task, &task);
                    set_prev(task, &task);
                } else {
                    uintptr_t deadline = task.deadline;
                    do {
                        int delta = deadline - t->deadline;
                        if (delta < 0) {
                            if (t == header)
                                header = &task;
                            break;
                        }
                        t = static_cast<task_t*>(get_next(*t));
                    } while (t != header);
                    task_t& prev = *static_cast<task_t*>(get_prev(*t));
                    set_next(prev, &task);
                    set_prev(*t, &task);
                    set_next(task, t);
                    set_prev(task, &prev);
                }
                return task;
            }
        };

        class sched_t : public co::sched_t {
        public:
            void create(task_t& task) __attribute__((alias("_ZN" __look_ns__ "3edf7sched_t8add_taskERNS0_6task_tE")));
            uintptr_t do_release(task_t& task, uintptr_t deadline);

        protected:                      // virtual
            base::task_t* ready(sync_t& sync, base::task_t* task = 0) override;
            void block(sync_t& sync) override;

        protected:
            task_t& add_task(task_t& task);
            void do_yield();

        protected:
            sync_t sync;
        };


        base::task_t* sched_t::ready(sync_t& sync, base::task_t* task)
        {
            task = sync.wait_queue.remove(static_cast<task_t*>(task));
            if (task != nullptr) {
                task_t* t = static_cast<task_t*>(ready_queue.get_header());
                task = &add_task(*static_cast<task_t*>(task));
                if (t != task)
                    need_sched = 1;
            }
            return task;
        }

        task_t& sched_t::add_task(task_t& task)
        {
            return static_cast<task_queue_t*>(&ready_queue)->insert(task);
        }

        void sched_t::block(sync_t& sync)
        {
            static_cast<task_queue_t&>(sync.wait_queue).insert(*static_cast<task_t*>(ready_queue.remove(static_cast<task_t*>(cur_task))));
        }

        uintptr_t sched_t::do_release(task_t& task, uintptr_t deadline)
        {
            if (!task.is_blocked() || task.context->except_saved.sync != &sync) {
                return 0;
            }
            task.deadline = deadline + get_systick().get_jiffies();
            do_wakeup(sync, &task, 0);
            return 1;
        }
    }

    namespace fp {                      // Fixed Priority Scheduling
        class task_t : public base::task_t {
            friend class sched_t;

        protected:
            uint8_t priority;
        };

        class sched_t : public base::sched_t {
        public:
            void create(task_t& task);
            __INLINE task_t* get_task(uint_fast8_t priority)
            {
                return task_table[priority];
            }

        protected:                  // virtual
            base::task_t* dispatch() override;
            base::task_t* ready(sync_t& sync, base::task_t* task_base = 0) override;
            void block(sync_t& sync) override;

        protected:
            task_t& add_task(task_t& task);

        private:
            bool do_set_priority(task_t& task, uint_fast8_t new_prio);

        protected:
            uintptr_t ready_map;
            task_t* task_table[sizeof(ready_map) * 8];
        };

        base::task_t* sched_t::dispatch()
        {
            return (ready_map != 0) ? task_table[__builtin_ctz(ready_map)] : static_cast<task_t*>(&idle_task);
        }

        base::task_t* sched_t::ready(sync_t& sync, base::task_t* task_base)
        {
            task_t* task = static_cast<task_t*>(task_base);
            if (task == nullptr) {
                if (sync.wait_map == 0)
                    return task;
                task = task_table[__builtin_ctz(sync.wait_map)];
            }
            sync.wait_map &= ~(1 << task->priority);
            return &add_task(*task);
        }

        task_t& sched_t::add_task(task_t& task)
        {
            uintptr_t mask = 1 << task.priority;
            ready_map |= mask;
            if ((ready_map & (mask - 1)) == 0)
                need_sched = 1;
            return task;
        }

        void sched_t::create(task_t& task)
        {
            task_table[task.priority] = &task;
            add_task(task);
        }

        void sched_t::block(sync_t& sync)
        {
            uintptr_t mask = 1 << static_cast<task_t*>(cur_task)->priority;
            sync.wait_map |= mask;
            ready_map &= ~mask;
        }

        bool sched_t::do_set_priority(task_t& task, uint_fast8_t new_prio)
        {
            if (new_prio >= sizeof(uintptr_t) * 8)
                return false;
            if (task_table[new_prio] != nullptr)
                return false;
            uint_fast8_t old_prio = task.priority;
            task_table[new_prio] = &task;
            task_table[old_prio] = nullptr;
            task.priority = new_prio;
            uintptr_t* p = &ready_map;
            if ((*p & (1 << old_prio)) != 0) {
                *p &= ~(1 << old_prio);
                *p |= 1 << new_prio;
                if ((*p & ((1 << static_cast<task_t*>(cur_task)->priority) - 1)) == 0)
                    need_sched = 1;
            } else {
                p = &task.context->except_saved.sync->wait_map;
                *p &= ~(1 << old_prio);
                *p |= 1 << new_prio;
            }
            return true;
        }

        class flag_t : public base::flag_t {
        public:
            bool do_set_bits(uintptr_t pattern);
        };

        bool flag_t::do_set_bits(uintptr_t pattern)
        {
            flags |= pattern;
            uintptr_t map = wait_map;
            while (flags != 0 && map != 0) {
                task_t* task = static_cast<sched_t&>(sched_t::get_scheduler()).get_task(__builtin_ctz(map));
                uintptr_t result = do_poll(task->context->except_saved.r1, mode_t(task->context->except_saved.r2));
                if (result != 0)
                    do_wakeup(result, task);
                map &= map - 1;
            }
            return true;
        }
    }

    namespace mlq {                     // Multi-Level Queue Scheduling
        class task_t : public link::task_t {
            friend class sched_t;
            friend class mutex_t;

        public:
            __INLINE uintptr_t get_current_priority() const
            {
                return priority.current;
            }

            __INLINE uintptr_t get_original_priority() const
            {
                return priority.original;
            }

        protected:
            void init(uintptr_t& context, uintptr_t prio);

        protected:
            union {
                struct {
                    uint8_t current;
                    uint8_t original;
                } priority;
                uint8_t priority8[2];
                uint16_t priority16;
            };
            uint8_t mutex_count;
        };

        using link::task_queue_t;

        class sched_t : public base::sched_t {
            friend class mutex_t;

        public:
            void create(task_t& task) __attribute__((alias("_ZN" __look_ns__ "3mlq7sched_t8add_taskERNS0_6task_tE")));
            bool do_yield(uintptr_t sched);

        protected:                                  // virtual
            base::task_t* dispatch() override;
            base::task_t* ready(sync_t& sync, base::task_t* task_base = 0) override;
            void block(sync_t& sync) override;

        private:
            void change_priority(task_t&, uintptr_t priority);
            task_t& add_task(mlq::task_t& task);
            task_t& remove_task(task_t& task);

        private:
            bool do_set_priority(task_t& task, uintptr_t new_prio);

        protected:
            uintptr_t ready_map;
            task_queue_t ready_queue[sizeof(ready_map) * 8];
        };

        base::task_t* sched_t::dispatch()
        {
            return ready_map != 0 ? static_cast<task_t*>(ready_queue[__builtin_ctz(ready_map)].get_header()) : static_cast<task_t*>(&idle_task);
        }

        base::task_t* sched_t::ready(sync_t& sync, base::task_t* task_base)
        {
            task_t* task = static_cast<task_t*>(task_base);
            task = static_cast<task_t*>(sync.wait_queue.remove(task));
            return task == 0 ? task : &add_task(*task);
        }

        task_t& sched_t::add_task(task_t& task)
        {
            uintptr_t prio = task.priority.current;
            if ((ready_map & ((1 << (prio + 1)) - 1)) == 0)
                need_sched = 1;
            ready_map |= 1 << prio;
            return static_cast<task_t&>(ready_queue[prio].enqueue(task));
        }

        task_t& sched_t::remove_task(task_t& task)
        {
            task_t* t = static_cast<task_t*>(ready_queue[task.priority.current].remove(&task));
            if (ready_queue[t->priority.current].get_header() == nullptr)
                ready_map &= ~(1 << t->priority.current);
            return *t;
        }

        void sched_t::block(sync_t& sync)
        {
            sync.wait_queue.enqueue(remove_task(*static_cast<task_t*>(cur_task)));
        }

        bool sched_t::do_set_priority(task_t& task, uintptr_t new_prio)
        {
            bool result = new_prio < sizeof(ready_map) * 8;
            if (result) {
                result = task.priority.original != new_prio;
                if (result) {
                    task.priority.original = new_prio;
                    change_priority(task, new_prio);
                }
            }
            return result;
        }

        bool sched_t::do_yield(uintptr_t sched)
        {
            auto& queue = ready_queue[static_cast<task_t*>(cur_task)->priority.current];
            task_t* task = static_cast<task_t*>(queue.get_header());
            if (task == cur_task) {
                if (task->get_next() != task) {
                    queue.rotate();
                    need_sched = sched;
                    return true;
                }
            }
            return false;
        }

        void sched_t::change_priority(task_t& task, uintptr_t priority)
        {
            if (task.priority.current == priority || (task.priority.current <= priority && task.mutex_count != 0))
                return;
            if (task.is_blocked()) {
                task.priority.current = priority;
                return;
            }
            task_t& t = remove_task(task);
            t.priority.current = priority;
            add_task(t);
            if ((ready_map & ((1 << static_cast<task_t*>(cur_task)->priority.current) - 1)) == 0)
                need_sched = 1;
        }

        class task_t;
        class cond_t;

        class mutex_t : public sync_t {
            friend cond_t;

        public:
            bool trylock();
            __INLINE uintptr_t lock(uintptr_t timeout = 0);
            __INLINE uintptr_t unlock();
            bool do_unlock();
            bool do_trylock(task_t& task);

        private:
            bool do_trylock();
            bool do_lock();
            bool do_release();

        private:
            task_t* owner;
            uintptr_t count;
        };

        bool mutex_t::do_trylock()
        {
            return do_trylock(static_cast<task_t&>(sched_t::get_scheduler().get_task()));
        }

        bool mutex_t::do_trylock(task_t& task)
        {
#if defined(__ARM_ARCH_6M__)
            task_t* _owner = owner;
            if (_owner == nullptr) {
                owner = &task;
                task.mutex_count++;
            } else if (_owner != &task)
                return false;
#elif defined(__ARM_ARCH_7M__)
            do {
                task_t* _owner;
                __ldrex(_owner, owner);
                if (_owner != nullptr) {
                    __clrex();
                    if (_owner == &task)
                        break;
                    return false;
                }
            } while (__extension__({
                    auto r = __strex(&task, owner);
                    if (r == 0)
                        task.mutex_count++;
                    r != 0;
                }));
#endif
            count++;
            return true;
        }

        bool mutex_t::do_lock()
        {
            task_t& task = static_cast<task_t&>(sched_t::get_scheduler().get_task());
            bool result = do_trylock(task);
            if (!result)
                static_cast<sched_t&>(sched_t::get_scheduler()).change_priority(*owner, task.priority.current);
            return result;
        }

        bool mutex_t::do_unlock()
        {
            task_t& task = static_cast<task_t&>(sched_t::get_scheduler().get_task());
            if (&task == owner) {
                if (--count == 0) {
                    uintptr_t mcnt = task.mutex_count;
                    task.mutex_count = --mcnt;
                    static_cast<sched_t&>(sched_t::get_scheduler()).change_priority(task, task.priority.original);
                    task_t* _owner = static_cast<task_t*>(do_wakeup(true));
                    owner = _owner;
                    if (_owner != nullptr) {
                        _owner->mutex_count++;
                        count = 1;
                    }
                }
                return true;
            }
            return false;
        }

        bool mutex_t::do_release()
        {
            while (do_wakeup(0));
            return true;
        }

        class cond_t : public sync_t {
        public:
            int wait(mutex_t& mutex, uintptr_t timeout);

            bool do_signal();
            bool do_broadcast();

            int do_wait(mutex_t& mutex);
            static bool do_relock(mutex_t& mutex);
        };

        int cond_t::wait(mutex_t& mutex, uintptr_t timeout)
        {
            register auto _this asm ("a1") = this;
            register auto _mutex1 asm ("a2") = &mutex;
            register auto _timeout1 asm ("a3") = timeout;
            int result = LOOK_SVC(2, int, &cond_t::do_wait, "0" (_this), "r" (_mutex1), "r" (_timeout1));

            register auto _mutex2 asm ("a1") = _mutex1;
            register auto _timeout2 asm ("a2") = 0;
            if (LOOK_SVC(1, int, &do_relock, "0" (_mutex2), "r" (_timeout2)) == 0)
                result = -2;
            return result;
        }

        int cond_t::do_wait(mutex_t& mutex)
        {
            bool result = mutex.do_unlock();
            sched_t::get_scheduler().lock_count += result;
            return result - 1;
        }

        bool cond_t::do_relock(mutex_t& mutex)
        {
            sched_t::get_scheduler().lock_count--;
            return mutex.do_lock();
        }

        bool cond_t::do_broadcast()
        {
            while (do_wakeup(1) != nullptr);
            return true;
        }
    }

    namespace mlqrr {
        class task_t : public mlq::task_t {
        };

        class sched_t : public mlq::sched_t {
        public:
            uintptr_t set_timeslice(uintptr_t interval);

        protected:                  // virtual
            void on_signal(void* addr, uintptr_t) override;
            base::task_t* dispatch() override;

        private:
            __INLINE void timer_restart(uintptr_t interval)
            {
                get_systick().do_set_timer(timer, interval);
            }
            __INLINE void timer_restart()
            {
                timer_restart(timeslice);
            }
            __INLINE void timer_stop()
            {
                timer_restart(0);
            }

        protected:
            timer_t timer;
            uintptr_t timeslice;
            uint16_t ts_save[32];
        };

        base::task_t* sched_t::dispatch()
        {
            task_t* task = static_cast<task_t*>(mlq::sched_t::dispatch());
            if (task != cur_task) {
                if (ready_map != 0) {
                    if (timer.is_active()) {
                        task_t& old_task = static_cast<task_t&>(*cur_task);
                        if (old_task.get_current_priority() < task->get_current_priority()) {       // block only one task
                            timer_restart(ts_save[task->get_current_priority()]);
                        } else {
                            if (old_task.get_current_priority() > task->get_current_priority()) {   // preemptive
                                ts_save[old_task.get_current_priority()] = get_systick().get_timer_remain(timer);
                            }                                                       // block or yield
                            timer_restart();
                        }
                    } else
                        timer_restart();
                } else
                    timer_stop();
            }
            return task;
        }

        void sched_t::on_signal(void* addr, uintptr_t)
        {
            if (addr == &timer)
                do_yield(1);
        }

        uintptr_t sched_t::set_timeslice(uintptr_t interval)
        {
            uintptr_t old = timeslice;
            if (interval != 0) {
                timeslice = interval;
                for (uintptr_t i = 0; i < 32; i++) {
                    uintptr_t data = ts_save[i];
                    if (data == 0 || data > interval)
                        ts_save[i] = interval;
                }
            }
            return old;
        }
    }

    namespace rr {
        class task_t : public co::task_t {
        };

        class sched_t : co::sched_t {
        public:
            uintptr_t set_timeslice(uintptr_t interval);

        protected:                  // virtual
            void on_signal(void* addr, uintptr_t) override;
            base::task_t* dispatch() override;

        private:
            __INLINE void timer_restart()
            {
                get_systick().do_set_timer(timer, timeslice);
            }

            __INLINE void timer_stop()
            {
                get_systick().do_set_timer(timer, 0);
            }

        protected:
            timer_t timer;
            uintptr_t timeslice;
        };

        void sched_t::on_signal(void* addr, uintptr_t)
        {
            if (addr == &timer)
                do_yield(1);
        }

        base::task_t* sched_t::dispatch()
        {
            base::task_t* task = co::sched_t::dispatch();
            if (task != cur_task) {
                if (task != &idle_task)
                    timer_restart();
                else
                    timer_stop();
            }
            return task;
        }

        uintptr_t sched_t::set_timeslice(uintptr_t interval)
        {
            uintptr_t old = timeslice;
            if (interval != 0) {
                timeslice = interval;
            }
            return old;
        }
    }
}
